/* ----------------------------------------------------------------------
*
* Copyright (C) 2025 XiaoFei. All rights reserved.
*
* @file:		XiaoZhi_VL53L0X.c
* @author:		XiaoFei
* @Date:     	01-March-2025
* @Revision: 	V1
* @Project: 	XiaoFei BSP Library
*
* -------------------------------------------------------------------- */

#include "stm32f4xx.h" 
#include "XiaoZhi_VL53L0X.h"
#include "XiaoFei_Delay.h"
#include "stdlib.h"

static unsigned char TunUse[]={
	0x01,0xFF,0x01,0x01,0x00,0x00,0x01,0xFF,0x00,0x01,0x09,0x00,0x01,0x10,0x00,0x01,0x11,0x00,
	0x01,0x24,0x01,0x01,0x25,0xff,0x01,0x75,0x00,0x01,0xFF,0x01,0x01,0x4e,0x2c,0x01,0x48,0x00,
	0x01,0x30,0x20,0x01,0xFF,0x00,0x01,0x30,0x09,0x01,0x54,0x00,0x01,0x31,0x04,0x01,0x32,0x03,
	0x01,0x40,0x83,0x01,0x46,0x25,0x01,0x60,0x00,0x01,0x27,0x00,0x01,0x50,0x06,0x01,0x51,0x00,
	0x01,0x52,0x96,0x01,0x56,0x08,0x01,0x57,0x30,0x01,0x61,0x00,0x01,0x62,0x00,0x01,0x64,0x00,
	0x01,0x65,0x00,0x01,0x66,0xa0,0x01,0xFF,0x01,0x01,0x22,0x32,0x01,0x47,0x14,0x01,0x49,0xff,
	0x01,0x4a,0x00,0x01,0xFF,0x00,0x01,0x7a,0x0a,0x01,0x7b,0x00,0x01,0x78,0x21,0x01,0xFF,0x01,
	0x01,0x23,0x34,0x01,0x42,0x00,0x01,0x44,0xff,0x01,0x45,0x26,0x01,0x46,0x05,0x01,0x40,0x40,
	0x01,0x0E,0x06,0x01,0x20,0x1a,0x01,0x43,0x40,0x01,0xFF,0x00,0x01,0x34,0x03,0x01,0x35,0x44,
	0x01,0xFF,0x01,0x01,0x31,0x04,0x01,0x4b,0x09,0x01,0x4c,0x05,0x01,0x4d,0x04,0x01,0xFF,0x00,
	0x01,0x44,0x00,0x01,0x45,0x20,0x01,0x47,0x08,0x01,0x48,0x28,0x01,0x67,0x00,0x01,0x70,0x04,
	0x01,0x71,0x01,0x01,0x72,0xfe,0x01,0x76,0x00,0x01,0x77,0x00,0x01,0xFF,0x01,0x01,0x0d,0x01,
	0x01,0xFF,0x00,0x01,0x80,0x01,0x01,0x01,0xF8,0x01,0xFF,0x01,0x01,0x8e,0x01,0x01,0x00,0x01,
	0x01,0xFF,0x00,0x01,0x80,0x00,0x00,0x00,0x00
};
static unsigned int Qua[4]={10,5,0,5};

/* 
* @function:    VL53L0X_SendByte
* @parameter:   [in] Byte				    The byte will be send from VL53L0X.
*/
static void VL53L0X_SendByte(unsigned char Byte)
{
	VL53L0X_SCL(0);
	for(unsigned char i=0;i<8;i++)
	{
		VL53L0X_SDA(!!((Byte&0x80)>>7));
		Byte<<=1;
		VL53L0X_SCL(1);
		VL53L0X_SCL(0);
	}
}

/* 
* @function: 	VL53L0X_ReadByte
* @return:		The data read from VL53L0X .
*/
static unsigned char VL53L0X_ReadByte(void)
{
	unsigned char Byte=0;
	for(unsigned char i=0;i<8;i++)
	{
		VL53L0X_SCL(0);
		VL53L0X_SCL(1);
		Byte<<=1;
		if(VL53L0X_SDARead)
			Byte++;
	}
	VL53L0X_SCL(0);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	VL53L0X_SCL(0);
	return Byte;
}

/* 
* @function: 	VL53L0X_WrByte
* @parameter:	[in] Reg				    The VL53L0X register to which the data will be sent.
* @parameter:	[in] Data				    The data will be send to VL53L0X reg.
*/
static void VL53L0X_WrByte(unsigned char Reg, unsigned char Data)
{
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	VL53L0X_SDA(0);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(VL53L0X_Address);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(Reg);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(Data);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	VL53L0X_SCL(0);
	VL53L0X_SDA(0);
	VL53L0X_SCL(1);
	VL53L0X_SDA(1);
}

/* 
* @function: 	VL53L0X_RdByte
* @parameter:	[in] Reg				    The VL53L0X register to which the data will be read.
* @parameter:	[out] Data				    The data will be read from VL53L0X reg.
*/
static void VL53L0X_RdByte(unsigned char Reg, unsigned char* Data)
{
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	VL53L0X_SDA(0);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(VL53L0X_Address);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(Reg);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	VL53L0X_SDA(0);
	VL53L0X_SCL(0);
	VL53L0X_SendByte(VL53L0X_Address|0x01);
	VL53L0X_SDA(1);
	VL53L0X_SCL(1);
	while(VL53L0X_SDARead);
	VL53L0X_SCL(0);
	*Data=VL53L0X_ReadByte();
	VL53L0X_SCL(0);
	VL53L0X_SDA(0);
	VL53L0X_SCL(1);
	VL53L0X_SDA(1);
}

/* 
* @function: 	VL53L0X_GetInfo
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
*/
static void VL53L0X_GetInfo(VL53L0X_TypeDef* Dev)
{
	unsigned char byte;
	VL53L0X_WrByte(0x80,0x01);
	VL53L0X_WrByte(0xFF,0x01);
	VL53L0X_WrByte(0x00,0x00);
	VL53L0X_WrByte(0xFF,0x06);
	VL53L0X_RdByte(0x83,&byte);
	VL53L0X_WrByte(0x83,byte|0x04);
	VL53L0X_WrByte(0xFF,0x07);
	VL53L0X_WrByte(0x81,0x01);
	for(unsigned int i=0;i<250;i++);
	VL53L0X_WrByte(0x80,0x01);
	VL53L0X_WrByte(0x94,0x24);
	for(unsigned char i=0;i<6;i++)
		VL53L0X_RdByte(0x90+i,&Dev->SpadMap[i]);
	VL53L0X_WrByte(0x81,0x00);
	VL53L0X_WrByte(0xFF,0x06);
	VL53L0X_RdByte(0x83,&byte);
	VL53L0X_WrByte(0x83,byte&0xfb);
	VL53L0X_WrByte(0xFF,0x01);
	VL53L0X_WrByte(0x00,0x01);
	VL53L0X_WrByte(0xFF,0x00);
	VL53L0X_WrByte(0x80,0x00);
}

/* 
* @function: 	VL53L0X_TunSet
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Tun				    The tunning data to be set.
* @return:		Return VL53L0X_ErrNone if the tunning is set successful.
*/
static VL53L0X_Error VL53L0X_TunSet(VL53L0X_TypeDef* Dev, unsigned char* Tun)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	int Index=0;
	unsigned char msb,lsb,sel,count,address;
	unsigned char local[4];
	while((*(Tun+Index)!=0)&&(Status==VL53L0X_ErrNone))
	{
		count=*(Tun+Index);
		Index++;
		if(count==0xFF)
		{
			sel=*(Tun+Index);
			Index++;
			if(sel==3)
			{
				msb=*(Tun+Index);
				Index++;
				lsb=*(Tun+Index);
				Index++;
				Dev->Rate=(((unsigned short int)msb)<<8)+(unsigned short int)lsb;
			}
			else if((sel==0)||(sel==1)||(sel==2))
				Index+=2;
			else
				Status=VL53L0X_InvPara;
		}
		else if(count<=4)
		{
			address=*(Tun+Index);
			Index++;
			for(unsigned char i=0;i<count;i++) 
			{
				local[i]=*(Tun+Index);
				Index++;
			}
			for(unsigned char i=0;i<count;i++)
				VL53L0X_WrByte(address+i,local[i]);
		} 
		else
			Status=VL53L0X_InvPara;
	}
	return Status;
}

/* 
* @function: 	VL53L0X_SetSeq
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
*/
static void VL53L0X_SetSeq(VL53L0X_TypeDef* Dev)
{
	unsigned char Seq=0;
	VL53L0X_RdByte(0x01,&Seq);
	Seq&=0xef;
	Seq&=0xfb;
	VL53L0X_WrByte(0x01,Seq);
	Dev->Sequence=Seq;
}

/* 
* @function: 	VL53L0X_StaticInit
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the static init is successful.
*/
static VL53L0X_Error VL53L0X_StaticInit(VL53L0X_TypeDef* Dev)
{
	Dev->SpadEnables=(unsigned char*)malloc(6*sizeof(unsigned char));
	Dev->SpadMap=(unsigned char*)malloc(6*sizeof(unsigned char));
	for(unsigned char i=0;i<6;i++)
	{
		Dev->SpadEnables[i]=0;
		Dev->SpadMap[i]=0;
	}
	VL53L0X_GetInfo(Dev);
	VL53L0X_Error Status=VL53L0X_TunSet(Dev,TunUse);
	VL53L0X_SetSeq(Dev);
	return Status;
}

/* 
* @function: 	VL53L0X_MeaPoll
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_MeaPoll(VL53L0X_TypeDef* Dev)
{
	unsigned char NewData=0;
	unsigned int LoopNb=0;
	do{
		VL53L0X_RdByte(0x14,&NewData);
		NewData=NewData&0x01;
		if(NewData==1)
			break;
		LoopNb++;
		if(LoopNb>=2000)
			return VL53L0X_TimeOut;
		for(unsigned int i=0;i<250;i++);
	}while(1);
	return VL53L0X_ErrNone;
}

/* 
* @function: 	VL53L0X_ClearInt
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_ClearInt(VL53L0X_TypeDef* Dev)
{
	unsigned char LoopCount=0;
	unsigned char Byte;
	do{
		VL53L0X_WrByte(0x0B,0x01);
		VL53L0X_WrByte(0x0B,0x00);
		VL53L0X_RdByte(0x13,&Byte);
		LoopCount++;
	}while(((Byte&0x07)!=0x00)&&(LoopCount<3));
	if(LoopCount>=3)
		return VL53L0X_InterCl;
	return VL53L0X_ErrNone;
}

/* 
* @function: 	VL53L0X_PerSingle
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerSingle(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	Status=VL53L0X_MeaPoll(Dev);
	if(Status==VL53L0X_ErrNone)
		Status=VL53L0X_ClearInt(Dev);
	if(Status==VL53L0X_ErrNone)
		VL53L0X_WrByte(0x00,0x00);
	return Status;
}

/* 
* @function: 	VL53L0X_PerRef
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerRef(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	unsigned char Seq=Dev->Sequence;
	VL53L0X_WrByte(0x01,0x01);
	VL53L0X_WrByte(0x00,0x41);
	Status=VL53L0X_PerSingle(Dev);
	if(Status==VL53L0X_ErrNone)
	{
		VL53L0X_WrByte(0x01,Seq);
		Dev->Sequence=Seq;
	}
	return Status;
}

/* 
* @function: 	VL53L0X_GetSpad
* @parameter:	[in] good				    The last good spad array.
* @parameter:	[in] curr				    The current spad index.
* @parameter:	[out] next				    The next good spad array.
*/
static void VL53L0X_GetSpad(unsigned char* good, unsigned int curr, int* next)
{
	unsigned int index;
	unsigned char success=0,dataByte;
	*next=-1;
	for(unsigned int i=(curr/8);((i<6)&&!success);i++)
	{
		index=0;
		dataByte=good[i];
		if(i==(curr/8))
		{
			dataByte>>=(curr%8);
			index=curr%8;
		}
		while(index<8)
		{
			if((dataByte&0x01)==1)
			{
				success=1;
				*next=i*8+index;
				break;
			}
			dataByte>>=1;
			index++;
		}
	}
}

/* 
* @function: 	VL53L0X_SpadEn
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] aper				    The aperture spads.
* @parameter:	[in] offset				    The offset.
* @parameter:	[out] last				    The last spad.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_SpadEn(VL53L0X_TypeDef* Dev,unsigned char aper, unsigned int offset,unsigned int* last)
{
	VL53L0X_Error status=VL53L0X_ErrNone;
	int Next=offset;
	unsigned int currentSpad=offset;
	unsigned char checkspad[6];
	unsigned char* Good=Dev->SpadMap;
	unsigned char* spad=Dev->SpadEnables;
	for(unsigned char index=0;index<3;index++)
	{
		VL53L0X_GetSpad(Good,currentSpad,&Next);
		if((Next==-1)||(!(!Qua[(0xB4+Next)>>6])!=aper))
		{
			status=VL53L0X_SpadInt;
			break;
		}
		currentSpad=(unsigned int)Next;
		spad[currentSpad/8]|=(1<<currentSpad%8);
		currentSpad++;
	}
	*last=currentSpad;
	if(status==VL53L0X_ErrNone)
		for(unsigned char i=0;i<6;i++)
			VL53L0X_WrByte(0xB0+i,spad[i]);
	for(unsigned char i=0;i<6;i++)
		VL53L0X_RdByte(0xB0+i,&checkspad[i]);	
	unsigned char i=0;
	while(i<6)
	{
		if(spad[i]!=checkspad[i])
		{
			status=VL53L0X_SpadInt;
			break;
		}
		i++;
	}
	return status;
}

/* 
* @function: 	VL53L0X_Start
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_Start(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	unsigned char Byte=0x01;
	uint32_t LoopNb=0;
	VL53L0X_WrByte(0x00,0x01);
	do{
		if(LoopNb>0)
			VL53L0X_RdByte(0x00,&Byte);
		LoopNb++;
	}while(((Byte&0x01)==0x01)&&(LoopNb<2000));
	if(LoopNb>=2000)
		Status=VL53L0X_TimeOut;
	return Status;
}

/* 
* @function: 	VL53L0X_PerMea
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerMea(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_Start(Dev);
	if(Status==VL53L0X_ErrNone)
		Status=VL53L0X_MeaPoll(Dev);
	if(Status==VL53L0X_ErrNone)
	{
		static unsigned char buf[2];
		for(unsigned char i=0;i<2;i++)
			VL53L0X_RdByte(i+0x1E,&buf[i]);
		Dev->Range=((unsigned short int)buf[0]<<8)+(unsigned short int)buf[1];
	}
	Status=VL53L0X_ClearInt(Dev);
	return Status;
}

/* 
* @function: 	VL53L0X_PerSig
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[out] refi				    The return data from VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerSig(VL53L0X_TypeDef* Dev, unsigned short int *refi)
{
	VL53L0X_Error status=VL53L0X_ErrNone;
	VL53L0X_WrByte(0x01,0xC0);
	status=VL53L0X_PerMea(Dev);
	if(status==VL53L0X_ErrNone)
	{
		VL53L0X_WrByte(0xFF,0x01);
		static unsigned char buf[2];
		for(unsigned char i=0;i<2;i++)
			VL53L0X_RdByte(0xB6+i,&buf[i]);
		*refi=((unsigned short int)buf[0]<<8)+(unsigned short int)buf[1];
		VL53L0X_WrByte(0xFF,0x00);
	}
	return status;
}

/* 
* @function: 	VL53L0X_PerSpad
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerSpad(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	unsigned char Last[6];
	unsigned int Cur=0,Lasts=0,Need=0,Sig=0,lastSig=0,ref=0;
	int Next=0;
	unsigned short int Tar=Dev->Rate,Peak;
	unsigned char complete=0;
	for(unsigned char index=0;index<6;index++)
		Dev->SpadEnables[index]=0;
	VL53L0X_WrByte(0xFF,0x01);
	VL53L0X_WrByte(0x4F,0x00);
	VL53L0X_WrByte(0x4E,0x2C);
	VL53L0X_WrByte(0xFF,0x00);
	VL53L0X_WrByte(0xB6,0xB4);
	VL53L0X_WrByte(0x80,0x00);
	Status=VL53L0X_PerRef(Dev);
	if(Status==VL53L0X_ErrNone)
		Status=VL53L0X_SpadEn(Dev,0,0,&Lasts);
	if(Status==VL53L0X_ErrNone)
	{
		Cur=Lasts;
		Status=VL53L0X_PerSig(Dev,&Peak);
		if((Status==VL53L0X_ErrNone)&&(Peak>Tar))
		{
			for(unsigned char index=0;index<6;index++)
				Dev->SpadEnables[index]=0;
			while((!(!Qua[(0xB4+Cur)>>6]))==0&&(Cur<44))
				Cur++;
			Need=1;
			Status=VL53L0X_SpadEn(Dev,1,Cur,&Lasts);
			if(Status==VL53L0X_ErrNone)
			{
				Cur=Lasts;
				Status=VL53L0X_PerSig(Dev,&Peak);
				if((Status==VL53L0X_ErrNone)&&(Peak>Tar))
					ref=3;
			}
		}
		else
			Need=0;
	}
	if((Status==VL53L0X_ErrNone)&&(Peak<Tar))
	{
		ref=3;
		for(unsigned char i=0;i<6;i++)
			Last[i]=Dev->SpadEnables[i];
		lastSig=(Peak-Tar>0)?(Peak-Tar):(Tar-Peak);
		complete=0;
		while(!complete)
		{
			VL53L0X_GetSpad(Dev->SpadMap,Cur,&Next);
			if(Next==-1)
			{
				Status=VL53L0X_SpadInt;
				break;
			}
			if((!(!Qua[(0xB4+Next)>>6]))!=Need)
			{
				complete=1;
				break;
			}
			(ref)++;
			Cur=Next;
			Dev->SpadEnables[Cur/8]|=(1<<Cur%8);
			if(Status==VL53L0X_ErrNone)
			{
				Cur++;
				for(unsigned char i=0;i<6;i++)
					VL53L0X_WrByte(0xB0+i,Dev->SpadEnables[i]);
			}
			if(Status!=VL53L0X_ErrNone)
				break;
			Status=VL53L0X_PerSig(Dev,&Peak);
			if(Status!=VL53L0X_ErrNone)
				break;
			Sig=(Peak-Tar>0)?(Peak-Tar):(Tar-Peak);
			if(Peak>Tar)
			{
				if(Sig>lastSig)
				{
					for(unsigned char i=0;i<6;i++)
						VL53L0X_WrByte(0xB0+i,Last[i]);
					for(unsigned int j=0;j<6;j++)
						Dev->SpadEnables[j]=Last[j];
					ref--;
				}
				complete=1;
			}
			else 
			{
				lastSig=Sig;
				for(unsigned int j=0;j<6;j++)
                    Last[j]=Dev->SpadEnables[j];
			}
		}
	}
	return Status;
}

/* 
* @function: 	VL53L0X_GetSeqEn
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[out] a				        The VL53L0X_Sche struct from VL53L0X.
*/
static void VL53L0X_GetSeqEn(VL53L0X_TypeDef* Dev, VL53L0X_Sche *a)
{
	unsigned char Seq=0;
	VL53L0X_RdByte(0x01,&Seq);
	a->TccOn=(Seq&0x10)>>4;
	a->DssOn=(Seq&0x08)>>3;
	a->MsrcOn=(Seq&0x04)>>2;
	a->PreRangeOn=(Seq&0x40)>>6;
	a->FinalRangeOn=(Seq&0x80)>>7;
}

/* 
* @function: 	VL53L0X_GetVcsel
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Type				    VCSEL period identifier.
* @parameter:	[out] a				        Pointer to VCSEL period value.
*/
static void VL53L0X_GetVcsel(VL53L0X_TypeDef* Dev, unsigned char Type, unsigned char* a)
{
	unsigned char reg;
	if(Type)
		VL53L0X_RdByte(0x70,&reg);
	else
		VL53L0X_RdByte(0x50,&reg);
	*a=(reg+1)<<1;
}

/* 
* @function: 	VL53L0X_GetSeqTim
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Id				        The sequence step Id.
* @parameter:	[out] Tim				    The Time Out Micro Secs.
*/
static void VL53L0X_GetSeqTim(VL53L0X_TypeDef* Dev,unsigned char Id,unsigned int* Tim)
{
	unsigned char Cur,Encode=0;
	unsigned short int PreRan=0,PreTim;
	VL53L0X_GetVcsel(Dev,0,&Cur);
	if(Id==2)
	{
		VL53L0X_RdByte(0x46,&Encode);
		PreTim=((unsigned int)(Encode&0x00FF)<<(unsigned int)((Encode&0xFF00)>>8))+1;
	}
	else if(Id==3)
	{
		unsigned char buf[2];
		for(unsigned char i=0;i<2;i++)
			VL53L0X_RdByte(0x51+i,&buf[i]);
		PreRan=((unsigned short int)buf[0]<<8)+(unsigned short int)buf[1];
		PreTim=((unsigned int)(PreRan&0x00FF)<<(unsigned int)((PreRan&0xFF00)>>8))+1;
	}
	*Tim=((PreTim*((unsigned int)(2304*Cur*1655)+500)/1000)+500)/1000;
}

/* 
* @function: 	VL53L0X_Encode
* @parameter:	[in] tim				    The timeout macro clks.
* @return:		Return the encode timeout.
*/
static unsigned short int VL53L0X_Encode(unsigned int tim)
{
	unsigned short int encoded=0,ms=0;
	unsigned int ls=0;
	if(tim>0) 
	{
		ls=tim-1;
		while((ls&0xFFFFFF00)>0) 
		{
			ls=ls>>1;
			ms++;
		}
		encoded=(ms<<8)+(uint16_t)(ls&0x000000FF);
	}
	return encoded;
}

/* 
* @function: 	VL53L0X_SetSeqTim
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Secs				    The Time Out Micro Secs.
*/
static void VL53L0X_SetSeqTim(VL53L0X_TypeDef* Dev, unsigned int Secs)
{
	unsigned char Cur;
	unsigned short int PreRan,PreTim=0,FinTim;
	unsigned int FinRan;
	VL53L0X_Sche Sch;
	VL53L0X_GetSeqEn(Dev,&Sch);
	if(Sch.PreRangeOn)
	{
		VL53L0X_GetVcsel(Dev,0,&Cur);
		static unsigned char buf[2];
		for(unsigned char i=0;i<2;i++)
			VL53L0X_RdByte(0x51+i,&buf[i]);
		PreRan=((unsigned short int)buf[0]<<8)+(unsigned short int)buf[1];
		PreTim=((unsigned int)(PreRan&0x00FF)<<(unsigned int)((PreRan&0xFF00)>>8))+1;
	}
	VL53L0X_GetVcsel(Dev,1,&Cur);
	FinRan=((Secs*1000)+((((unsigned int)(2304*Cur*1655)+500)/1000)/2))/(((unsigned int)(2304*Cur*1655)+500)/1000);
	FinRan+=PreTim;
	FinTim=VL53L0X_Encode(FinRan);			
	VL53L0X_WrByte(0x71,(unsigned char)(FinTim>>8));
	VL53L0X_WrByte(0x72,(unsigned char)(FinTim&0xFF));
}

/* 
* @function: 	VL53L0X_SetSecond
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Mood				    This parameter can be any value of @ref VL53L0X_MoodDef.
* @return:		Return VL53L0X_ErrNone if the mood is set successful.
*/
static VL53L0X_Error VL53L0X_SetSecond(VL53L0X_TypeDef* Dev, VL53L0X_MoodDef Mood)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	unsigned int Fin,MDT=2000,Pre=0,Sub=0;
	VL53L0X_Sche Sch;
	Fin=Mood-(1910+960);
	VL53L0X_GetSeqEn(Dev,&Sch);
	if(Sch.TccOn||Sch.MsrcOn||Sch.DssOn)
	{
		VL53L0X_GetSeqTim(Dev,2,&MDT);
		if(Sch.TccOn)
			Sub=MDT+590;
		if(Sch.DssOn)
			Sub=2*(MDT+690);
		if(Sch.MsrcOn)
			Sub=MDT+660;
		if(Sub<Fin)
            Fin-=Sub;
		else
            Status=VL53L0X_InvPara;
	}
	if(Status!=VL53L0X_ErrNone)
		return Status;
	if(Sch.PreRangeOn)
	{
		VL53L0X_GetSeqTim(Dev,3,&Pre);
		Sub=Pre+660;
		if(Sub<Fin)
			Fin-=Sub;
		else
			Status=VL53L0X_InvPara;
	}
	if(Status==VL53L0X_ErrNone&&Sch.FinalRangeOn)
	{
		Fin-=550;
		VL53L0X_SetSeqTim(Dev,Fin);
	}
	return Status;
}

/* 
* @function: 	VL53L0X_PerfPhase
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @return:		Return VL53L0X_ErrNone if the operation is successful.
*/
static VL53L0X_Error VL53L0X_PerfPhase(VL53L0X_TypeDef* Dev)
{
	VL53L0X_Error Status=VL53L0X_ErrNone;
	unsigned char Seq=Dev->Sequence;
	VL53L0X_WrByte(0x01,0x02);
	VL53L0X_WrByte(0x00,0x01);
	Status=VL53L0X_PerSingle(Dev);
	if(Status==VL53L0X_ErrNone)
	{
		VL53L0X_WrByte(0x01,Seq);
		Dev->Sequence=Seq;
	}
	return Status;
}

/* 
* @function: 	VL53L0X_SetMood
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[in] Mood				    This parameter can be any value of @ref VL53L0X_MoodDef.
* @return:		Return VL53L0X_ErrNone if the mood is set successful.
*/
VL53L0X_Error VL53L0X_SetMood(VL53L0X_TypeDef* Dev, VL53L0X_MoodDef Mood)
{
	VL53L0X_Error Status=VL53L0X_StaticInit(Dev);
	if(Status!=VL53L0X_ErrNone)
		return Status;
	XiaoFei_Delay_ms(2);
	Status=VL53L0X_PerRef(Dev);
	if(Status!=VL53L0X_ErrNone)
		return Status;
	XiaoFei_Delay_ms(2);
	Status=VL53L0X_PerSpad(Dev);
	if(Status!=VL53L0X_ErrNone)
		return Status;
	XiaoFei_Delay_ms(2);
	VL53L0X_SetSecond(Dev,Mood);
	VL53L0X_WrByte(0x57,0x30);
	VL53L0X_WrByte(0x56,0x08);
	VL53L0X_WrByte(0x50,0x06);
	Status=VL53L0X_PerfPhase(Dev);
	return Status;
}

/* 
* @function: 	VL53L0X_GetData
* @parameter:	[in] Dev				    The Device handle of VL53L0X.
* @parameter:	[out] buf				    The distance obtained is measured.
* @return:		Return VL53L0X_ErrNone if the measure is set successful.
*/
VL53L0X_Error VL53L0X_GetData(VL53L0X_TypeDef* Dev, unsigned int *buf)
{
	*buf=0;
	VL53L0X_Error status=VL53L0X_PerMea(Dev);
	if(status!=VL53L0X_ErrNone)
		return status;
	if((Dev->Range>1200)||(Dev->Range<50))
		return VL53L0X_RangOut;
	*buf=Dev->Range;
	return VL53L0X_ErrNone;
}

/* 
* @function: 	VL53L0X_Init
* @parameter:	[out] Dev				    The Device handle of VL53L0X.
*/
void VL53L0X_Init(VL53L0X_TypeDef* Dev)
{
	VL53L0X_XSH_RCC(1);
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT ;		
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;	
	GPIO_InitStructure.GPIO_Pin = VL53L0X_PIN_XSH;				
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;		
	GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
	GPIO_Init(VL53L0X_GPIO_XSH,&GPIO_InitStructure);
	VL53L0X_SDA_RCC(1);
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType=GPIO_OType_OD;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Pin=VL53L0X_PIN_SDA;
	GPIO_Init(VL53L0X_GPIO_SDA,&GPIO_InitStructure);
	VL53L0X_SCL_RCC(1);
	GPIO_InitStructure.GPIO_Pin=VL53L0X_PIN_SCL;
	GPIO_Init(VL53L0X_GPIO_SCL,&GPIO_InitStructure);
	VL53L0X_XSH(0);
	XiaoFei_Delay_ms(30);
	VL53L0X_XSH(1);
	XiaoFei_Delay_ms(30);
	VL53L0X_WrByte(0x88,0x00);
	VL53L0X_WrByte(0x44,0x00);
	VL53L0X_WrByte(0x45,0x20);
	VL53L0X_WrByte(0x01,0xFF);
}

/* 
* @function: 	VL53L0X_Free
* @parameter:	[in] Dev				    The Device handle of VL53L0X will be free.
*/
void VL53L0X_Free(VL53L0X_TypeDef* Dev)
{
	free(Dev->SpadEnables);
	free(Dev->SpadMap);
}


